#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <control_msgs/FollowJointTrajectoryAction.h>

typedef actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> TrajClient;

int main(int argc, char **argv)
{
      ros::init(argc, argv, "gazebo_arm_control");
      ros::NodeHandle nh;

      // 1. 创建 Action 客户端（注意命名空间）
      TrajClient traj_client("arm_7dof/arm_controller/follow_joint_trajectory", true);
      traj_client.waitForServer();

      // 2. 构造轨迹目标
      control_msgs::FollowJointTrajectoryGoal goal;
      goal.trajectory.joint_names = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7"};

      // 3. 添加轨迹点（示例：2秒内移动到目标位置）
      trajectory_msgs::JointTrajectoryPoint point;
      point.positions = {0.0, 0, 0, 1.57, 0, 0, 0.0}; // 7个关节的目标角度（弧度）
      point.time_from_start = ros::Duration(2.0);
      goal.trajectory.points.push_back(point);

      // 4. 发送轨迹
      traj_client.sendGoal(goal);
      ROS_INFO("Trajectory sent to Gazebo!");

      // 5. 等待执行完成
      if (traj_client.waitForResult(ros::Duration(5.0)))
      {
            ROS_INFO("Execution succeeded!");
      }
      else
      {
            ROS_ERROR("Execution failed!");
      }

      return 0;
}